#pragma config(Sensor, in1,    armPot,              sensorPotentiometer)
#pragma config(Sensor, in2,    clawPot,             sensorPotentiometer)
#pragma config(Sensor, in3,    LineFollowTL,        sensorNone)
#pragma config(Sensor, in4,    LineFollowTR,        sensorNone)
#pragma config(Sensor, in5,    LineFollowC,         sensorNone)
#pragma config(Sensor, in6,    LineFollowBL,        sensorNone)
#pragma config(Sensor, in7,    LineFollowBR,        sensorNone)
#pragma config(Sensor, dgtl1,  rightEnc,            sensorQuadEncoder)
#pragma config(Sensor, dgtl3,  leftEnc,             sensorQuadEncoder)
#pragma config(Motor,  port1,           backleft,      tmotorNormal, openLoop)
#pragma config(Motor,  port2,           left,          tmotorNormal, openLoop)
#pragma config(Motor,  port3,           armright1,     tmotorNormal, openLoop)
#pragma config(Motor,  port4,           armleft1,      tmotorNormal, openLoop, reversed)
#pragma config(Motor,  port5,           claw,          tmotorNormal, openLoop)
#pragma config(Motor,  port7,           armleft2,      tmotorNormal, openLoop, reversed)
#pragma config(Motor,  port8,           armright2,     tmotorNormal, openLoop)
#pragma config(Motor,  port9,           right,         tmotorNormal, openLoop, reversed)
#pragma config(Motor,  port10,          backright,     tmotorNormal, openLoop)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

/*
  LineFollowBL, LineFollowBR, LineFollowC, lineFollowTL, LineFollowTR are all sensors for following the line with the sensors set up in an "X" configuration. The "X" configuration is for more
  advanced line following and until we need it, only three will be used in normal line following (lineFollowTL, LineFollowC, and LineFollowTR
*/

#pragma platform(VEX)

//Competition Control and Duration Settings
#pragma competitionControl(Competition)
#pragma autonomousDuration(20)
#pragma userControlDuration(120)

#include "Vex_Competition_Includes.c"   //Main competition background code...do not modify!
#include "PIDController.c"

/****************************************************************************************DRIVING FUNCTIONS*****************************************************************************************/
int leftDrivePower, rightDrivePower, x, y, rampLimit = 2, leftDrivePrevious, rightDrivePrevious;//drivetrain variables


void resetDriveVariables(){
  leftDrivePower = rightDrivePower = x = y = leftDrivePrevious = rightDrivePrevious = 0;
}

void setDriveMotors(){
  motor[left] = leftDrivePower;
  motor[backleft] = leftDrivePower;
  motor[right] = rightDrivePower;
  motor[backright] = rightDrivePower;
}

void stopDrive(){
  rightDrivePower = leftDrivePower = 0;
  setDriveMotors();
}

void driveTank(int left, int right, bool square, bool ramp){
  if(square){
    if(left!=0){
      leftDrivePower = ((left*left)/127)*(left/abs(left));
    }
    else{
      leftDrivePower = 0;
    }

    if(right!=0){
      rightDrivePower = ((right*right)/127)*(right/abs(right));
    }
    else{
      rightDrivePower = 0;
    }
  }
  else{
    leftDrivePower = left;
    rightDrivePower = right;
  }

  if(ramp == true){
    if(abs(rightDrivePower) > abs(rightDrivePrevious) + rampLimit){
      if(rightDrivePower > (rightDrivePower + rampLimit)){
        rightDrivePower = rightDrivePrevious + rampLimit;
      }
      else{
        rightDrivePower = rightDrivePrevious - rampLimit;
      }
    }

    if(abs(leftDrivePower) > abs(leftDrivePrevious) + rampLimit){
      if(leftDrivePower > (leftDrivePower + rampLimit)){
        leftDrivePower = leftDrivePrevious + rampLimit;
      }
      else{
        leftDrivePower = leftDrivePrevious - rampLimit;
      }
    }
  }

  setDriveMotors();
}

void driveArcade(int power, int turn, bool square, bool ramp){
  if(square){
    if(power != 0){
      y = ((power*power)/127)*(power/abs(power));
    }
    else{
      y = 0;
    }

    if(turn!=0){
      x = ((turn*turn)/127)*(turn/abs(turn));
    }
    else{
      x = 0;
    }
  }
  else{
    y = power;
    x = turn;
  }

  leftDrivePower = y+x;
  rightDrivePower = y-x;

  if(ramp == true){
    if(abs(rightDrivePower) > abs(rightDrivePrevious) + rampLimit){
      if(rightDrivePower > (rightDrivePower + rampLimit)){
        rightDrivePower = rightDrivePrevious + rampLimit;
      }
      else{
        rightDrivePower = rightDrivePrevious - rampLimit;
      }
    }

    if(abs(leftDrivePower) > abs(leftDrivePrevious) + rampLimit){
      if(leftDrivePower > (leftDrivePower + rampLimit)){
        leftDrivePower = leftDrivePrevious + rampLimit;
      }
      else{
        leftDrivePower = leftDrivePrevious - rampLimit;
      }
    }
  }

  setDriveMotors();

  leftDrivePrevious = leftDrivePower;
  rightDrivePrevious = rightDrivePower;
}
/*******************************************************************************************Arm & Claw Code****************************************************************************************/

/*********PID Variables****************/
/**************************************/
PIDController Arm;
PIDController Claw;

int ArmSetPoint;
int ArmKp = 10,ArmKi = 0, ArmKd = 0;

int ClawSetPoint;
int ClawKp = 10, ClawKi = 0, ClawKd = 0;
/**************************************/
/**************************************/

void setArmPids(kp, ki, kd){
  ArmKp = kp;
  ArmKi = ki;
  ArmKd = kd;
  setPIDs(Arm, ArmKp, ArmKi, ArmKd);
}

void setClawPids(kp, ki, kd){
  ClawKp = kp;
  ClawKi = ki;
  ClawKd = kd;
  setPIDs(Claw, ClawKp, ClawKi, ClawKd);
}

int armPower, clawPower, armPosition, clawPosition, armTolerance, clawTolerance, initialArmPosition, initialClawPosition, armOffset, clawOffset;//arm and claw tolerance to be set
int armFloor = 2213, armLowPlace = 3079, armLowScore = 2472, armHighPlace = 3303, armHighScore = 2872, armMax = 3845, armSetPoint, armState = 0;//arm positions. Place is position above goal, Score is on goal for tubes, floor is start position
int clawOpen = 2100, clawClosed = 3500, clawGuiding = 2900;//claw positions. Open is initial position, close is closed on tubes, Guiding is open slightly to better align on tubes for close
int armInput, clawInput, armPreviousInput, armRampLimit = 2, rampClawLimit = 0.2;//arm variables used in competitions code. Placed here to keep them with the rest of the arm variables
bool armPositionReached, clawPositionReached, slowClose = false;

/*
  setInitialPosition() is used to save the beginning values of the arm and claw at the start of the program. The program will assume that the arm and claw start out with the arm at the lowest
  point and the claw to be fully open. This will allow for relative positions of the arm and claw throughout programs to account for any possible shift in the potentiometers.
  Possible addition is to add a limitswitch to the bar that is the physical limit for the arm to reset the initial position of the arm whenever the arm is fully lowered to account for any
  possible shift of the potentiometer during the match. (STILL TO BE DONE, NOT NECESSARY BUT WOULD BE NICE)
*/

/*void setInitialPosition(){
  initialArmPosition = SensorValue[armPot];
  initialClawPosition = SensorValue[clawPot];

  armOffset = armFloor - initialArmPosition;
  clawOffset = clawOpen - initialClawPosition;
}

void setArmPosition(){
  initialArmPosition = SensorValue[armPot];

  armOffset = armFloor - initialArmPosition;
}

void setArmMotors(){
  if(abs(armPower) > abs(armPreviousInput)+armRampLimit){
    if(armPower > armPreviousInput + armRampLimit){
      armPower = armPreviousInput + armRampLimit;
    }
    else if(armPower < armPreviousInput - armRampLimit){
      armPower = armPreviousInput - armRampLimit;
    }
  }
  motor[armleft1] = motor[armleft2] = motor[armright1] = motor[armright2] = armPower;
  motor[claw] = clawPower;
}

bool armMoveAuton(int aPower, int cPower, int armPos, int clawPos){//basic controls right now. Should be replaced with a proper PID loop control on the arm position when the programmer isn't tired.
  if((SensorValue[armPot] <= (armPos - armTolerance))||(SensorValue[armPot] >= (armPos + armTolerance))){
    armPower = ((armPos - armPot)/armPos) * aPower;
    armPositionReached = false;
  }
  else{
    armPower = 0;
    armPositionReached = true;
  }

  if((SensorValue[clawPot] <= (clawPos - clawTolerance))||(SensorValue[clawPot] >= (clawPos + clawTolerance))){
    clawPower = ((clawPos - clawPot)/clawPos) * cPower;
    clawPositionReached = false;
  }
  else{
    clawPower = 0;
    clawPositionReached = true;
  }

  setArmMotors();
  return clawPositionReached && armPositionReached;
}

void armMoveTeleop(int cPower, int armPos, int clawPos){

  if((SensorValue[armPot] <= (armPos - armTolerance))||(SensorValue[armPot] >= (armPos + armTolerance))){
    armPower = ((armPos - armPot)/armPos) * 128;
  }
  else{
    armPower = 0;
  }

  if((SensorValue[clawPot] <= (clawPos - clawTolerance))||(SensorValue[clawPot] >= (clawPos + clawTolerance))){
    clawPower = ((clawPos - clawPot)/clawPos) * cPower;
  }
  else{
    clawPower = 0;
  }

  setArmMotors();
}

void armMove(int pos){
  if(SensorValue[armPot] < pos){
    armPower = 127;
  }
  else if(SensorValue[armPot] > pos){
    armPower = -20;
  }
  else{
    armPower = 0;
  }
}

void armMoveTeleop(int cPower, int armPos){

  /*if((SensorValue[armPot] <= (armPos - armTolerance))||(SensorValue[armPot] >= (armPos + armTolerance))){
    armPower = ((armPos - armPot)/armPos) * 128;
  }
  else{
    armPower = 0;
  }

  if(SensorValue[armPot] > armPos){
    armPower = 50;
  }
  else if(SensorValue[armPot] < armPos){
    armPower = -128;
  }
  clawPower = cPower;

  setArmMotors();
}
*/

void setArmMotors(){
  /*if(abs(armPower) > abs(armPreviousInput)+armRampLimit){
    if(armPower > armPreviousInput + armRampLimit){
      armPower = armPreviousInput + armRampLimit;
    }
    else if(armPower < armPreviousInput - armRampLimit){
      armPower = armPreviousInput - armRampLimit;
    }
  }
  */
  motor[armleft1] = motor[armleft2] = motor[armright1] = motor[armright2] = armPower;
  motor[claw] = clawPower;
}

void armMove(int aPower, int cPower){
  armPower = aPower;
  clawPower = cPower;
  setArmMotors();
}


/*****************************************************************************************Competition Code*****************************************************************************************/
void pre_auton()
{
  //init(Arm,1);
}

task autonomous()
{
  /*bool reached = false;
  setArmPids(ArmKp, ArmKi, ArmKd);
  setMaxError(Arm, 10);
  setOutputRange(Arm, -20, 127);
  enable(Arm);
  while(!reached){

    setSetpoint(Arm, armLowPlace);

    if(onTarget(Arm) == true){
      reached = true;
    }
  }

  disable(Arm);
  */
  while(sensorValue[clawPot] < clawClosed){
    clawPower = 128;
    armMove(0,clawPower);
  }

  while(sensorValue[armPot] < armLowPlace){
    armPower = 64;
    armMove(armpower,0);
  }
  armMove(0,0);

  time1[T1] = 0;
  while(time1[T1] < 500){
    driveArcade(32,0,false,true);
  }
  driveArcade(0,0,false,false);
}

task usercontrol()
{
  resetDriveVariables();
  armState = 0;
	while (true)
	{
    driveArcade(vexRT[Ch3],vexRT[Ch4],true,true);//leave both square and ramp as false until the code is proven to work

    if (vexRT[Btn6U] != 0){
      clawInput = 128;
      slowClose = false;
    }
    else if(vexRT[Btn6D] != 0){
      clawInput = -63;
      slowClose = false;
    }
    else if(vexRT[Btn5U] != 0){
      clawInput = 40;
      slowClose = true;
    }
    else if(vexRT[Btn5D]){
      clawInput = -31;
      slowClose = true;
    }
    else{
      clawInput = 0;
      slowClose = false;
    }

    armMove(vexRT[Ch2],clawInput);
    /*
    if(abs(vexRT[Ch2]) >= 10){
      armPower = vexRT[Ch2];
      disable(Arm);
    }
    else if(vexRT[Btn8D]!= 0 || vexRT[Btn8R] != 0){
      enable(Arm);
      switch(armState){
        case 0://arn us at floor
          setSetpoint(Arm,armFloor);
          if(vexRT[Btn8D] != 0){
            armState = 1;//moves to place on low goal
          }
          else if(vexRT[Btn8R] != 0){
            armState = 3;//moves to place on high goal
          }
          break;

        case 1://placing on low goal
          setSetpoint(Arm,armLowPlace);
          if(vexRT[Btn8D] != 0){
            armState = 2;//move to score on low goal
          }
          else if(vexRT[Btn8R] != 0){
            armState = 3;//move to place on high goal
          }
          break;

        case 2://scoring on low goal
          setSetpoint(Arm,armLowScore);
          if(vexRT[Btn8D] != 0){
            armState = 0;//move to floor position
          }
          else if(vexRT[Btn8R] != 0){
            armState = 1;//return to placing position of low goal
          }
          break;

        case 3://placing position of high goal
          setSetpoint(Arm, armHighPlace);
          if(vexRT[Btn8D] != 0){
            armState = 1;//moving to place on low goal
          }
          else if(vexRT[Btn8R] != 0){
            armState = 4;//moving to score on high goal
          }
          break;

        case 4://scoring position of high goal
          setSetpoint(Arm, armHighScore);
          if(vexRT[Btn8D] != 0){
            armState = 3;//move to placing position of high goal
          }
          else if(vexRT[Btn8R] != 0){
            armState = 0;//move to floor
          }
        break;

        default:
          armMove(0,0);
      }


    }
    else{
      disable(Arm);
      armMove(0,0);
    }
    */
  }
}
